import math

import thread
import threading
from naoqi import ALModule
from naoqi import ALBroker
from naoqi import ALProxy
import sys
import time

NAO_IP = "192.168.0.100"
NAO_PORT = 9559
myBroker1 = ALBroker("myBroker",
                     "0.0.0.0",  # listen to anyone
                     0,  # find a free port and use it
                     NAO_IP,  # parent broker IP
                     NAO_PORT)
sonarproxy = ALProxy("ALSonar")
sonarproxy.subscribe("myApplication")
memorypxy = ALProxy("ALMemory")
alproxy = ALProxy("ALMotion")

tts = ALProxy("ALTextToSpeech")
posture = ALProxy("ALRobotPosture")

h = 0.5  # dis from obstacle
x = 1  # dis from sonar
a = 1  # degree
newX = x - h * math.cos(a)
time = 1
speed_X = (math.pi - 2 * a) / (2 * math.cos(a)) * newX / time
speed_Z = (math.pi - 2 * a) / time
